using System;
using GT = Gadgeteer;
using Gadgeteer.Interfaces;
using Gadgeteer.Modules.GHIElectronics;

namespace Coding4Fun.BluetoothPanoHead
{
    /// <summary>
    /// An class for controlling a Parallax (Futaba S148) Continuous Rotation Servo
    /// </summary>
    public class ServoControl
    {
        readonly PWMOutput _servo;

        readonly uint _period;
        readonly uint _centerStop;

        private int _speed;
       
        public ServoControl(Extender extender, GT.Socket.Pin servoPin)
        {
            // Set the specified pin to PWM mode
            _servo = extender.SetupPWMOutput(servoPin);

            // pulse period (20ms)
            _period = 20 * (1000 * 1000);

            // 0 speed value of 1.5ms
            _centerStop = 1500 * 1000;

            // Default speed
            _speed = 10;
        }

        /// <summary>
        /// Gets or sets the speed of the servo.
        /// </summary>
        /// <value>
        /// The speed.
        /// </value>
        public int Speed
        {
            get { return _speed; }
            set
            {
                int absValue = Math.Abs(value);
                if ((absValue > 0) && (absValue < 100))
                    _speed = value;
            }
        }

        /// <summary>
        /// Convert a direction value to the units used by the servo
        /// </summary>
        /// <param name="value">Direction.</param>
        /// <returns></returns>
        public uint Direction(int value)
        {
            return (uint)((1500 + value) * 1000);
        }

        /// <summary>
        /// Stops the servo from turning by turning off the PWM signal.
        /// </summary>
        public void Stop()
        {
            _servo.Active = false;
        }

        /// <summary>
        /// Sends the signal to the servo for adjusting the set point.  The servo control needs to get a 1.5ms pulse
        /// at 20ms intervals
        /// </summary>
        public void Calibrate()
        {
            _servo.Active = true;
            _servo.SetPulse(_period, _centerStop);
        }

        /// <summary>
        /// Turn in the default direction at the default direction and speed
        /// </summary>
        public void Turn()
        {
            Turn(_speed);
        }

        /// <summary>
        /// Tell the servo to turn with the specified direction and speed
        /// </summary>
        /// <param name="speed">Tell the servo to start rotating.  Left is LT 0, right is GT 0></param>
        public void Turn(int speed)
        {
            _servo.Active = true;
            _servo.SetPulse(_period, Direction(speed));
        }
    }
}
